#pragma once

#include "rclcpp/rclcpp.hpp"
#include <cmath>
#include <string>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_srvs/srv/follow_trigger.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
// #include <canopen_chain_node/SetObject.h>
#include <agv_peripheral_controller/amr/peripheral.h>

#define JACKING_JOINT "jacking_joint"
#define ROTARY_JOINT "rotary_joint"
#define PROVELOBJ "6081"
#define BOUND 1e-2


namespace agv_peripheral 
{

    double getRemainder(double x, double y);
    double getErr(double x, double y);
    double normalizationRad(double rad);

	class PalletController 
	{
	public:

		enum TYPE 
		{
			IDLE,
			FOLLOW,
			FIX,
			RESET,
			TASK
		} state_type;

		explicit PalletController(std::shared_ptr<rclcpp::Node> n);
		~PalletController(){};

		bool init();
		int get_state() { return state;};
		int set_rotary_target_position(double target);
		int set_rotary_target_velocity(double target);
		int set_jacking_target_position(double target);

		void set_followup(int followup) { follow_onoff = followup;};
        void set_forbid_rotary(int forbid) { rotary_forbid = forbid;};
        void set_manual_mode(bool mode) { manual_mode = mode;};
		void update();

	private:
		int 	state = 0;
		int 	follow_onoff = 0;
        int     rotary_forbid = 0;
        int     last_rotary_forbid = 0;
		bool  	jacking_up_flag;
        bool    jacking_down_flag;
		int  	last_state;
        int  	last_follow_onoff = 0;
        bool 	last_jacking_up_flag = false;
        bool 	manual_mode = true;

        bool 	find_joint_id = false;
        int 	jacking_id;
        int 	rotary_id;
        double 	jacking_min_position;
        double 	jacking_max_position;
        double 	jacking_position;
        double 	rotary_position;
        double 	current_pallet_angular_speed;
        int     reset_scale;
        double  rotary_multiplier;
        bool    rotary_fix_enable = false;
        double  rotary_fix_threshold = 0.1;


        double 	roll, pitch, yaw;
        double 	target_yaw;
        double 	current_chassis_yaw;
        double 	pallet_yaw;
        double 	pallet_yaw_err[5];
        double 	pallet_yaw_integral;
        double 	current_chassis_linear_speed;
        double 	current_chassis_angular_speed;
        double 	current_chassis_angular_speed_target;

        double 	target_pallet_position = 0;

        rclcpp::Time time_now;
        rclcpp::Time fix_start_time;
        rclcpp::Time acc_start_time;

        rclcpp::Time cmd_vel_update_time;
        double rotary_angular_control_kp = 2.0;
        double rotary_angular_control_ki = 0.05;
        double rotary_angular_control_kd = 0.2;
        double feedforward_comp_coeff = 0.8;
        double rotary_angular_control_acc = 0.8;

        double rotary_fix_speed = 0.1;
        double rotary_reset_speed = 0.8;
        double rotary_task_speed = 0.8;

        bool jacking_target_updated = false;

        std::shared_ptr<rclcpp::Node> node;
        rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr rotary_pub;
        rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr jacking_pub;
        rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odometry_sub;
        rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_sub;
        rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_sub;
        rclcpp::Service<agv_srvs::srv::FollowTrigger>::SharedPtr trigger_service;
        geometry_msgs::msg::Pose 		  odom_pose;
        sensor_msgs::msg::JointState 	  joint_state;
        std_msgs::msg::Float64MultiArray  rotary_angular_speed_target;
        std_msgs::msg::Float64MultiArray  jacking_position_target;

        void 	odometrycallback(const agv_msgs::msg::OdometryLite::ConstSharedPtr& msg);
        void 	cmdvelcallback(const geometry_msgs::msg::TwistStamped& cmd);
        void 	jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg);
		bool 	follow_triggercallback(const std::shared_ptr<agv_srvs::srv::FollowTrigger::Request> request,
                    std::shared_ptr<agv_srvs::srv::FollowTrigger::Response> response);
        double 	getPalletYaw(double chassis_yaw, double rotary_position);
        double 	getRadDistance(double target, double current, bool absolute = false);
        void 	rotaryControl(double target, double current, int current_state);
		void 	rotaryFollow();
        void    execute_jacking_operation();


	};
}